//final test
package org._649mset.omnibot;

import edu.wpi.first.wpilibj.*;

public class HolonomicDT {
    public final int VICTOR_COUNT = 4;
    
    private final double[] pseudoDT;
    private final Victor[] DT;
    
    public HolonomicDT(int[] dt) {
    	pseudoDT = new double[VICTOR_COUNT];
    	DT = new Victor[VICTOR_COUNT];
    	for(int i = 0; i < VICTOR_COUNT; i++) {
    		pseudoDT[i] = 0;
    		DT[i] = new Victor(dt[i]);
    	}
    }

    public double[] getPseudoDT() {
    	return pseudoDT;
    }
    public Victor[] getDT() {
    	return DT;
    }
    
    public void drive(double y, double x, double rot) {
    	pseudoDT[0] = y - x - rot;
        pseudoDT[1] =  y + x - rot;
        pseudoDT[2] = -y + x - rot;
        pseudoDT[3] = -y - x - rot;
        double max = pseudoDT[0];
        for(int i = 1;i < VICTOR_COUNT; i++) {
            if(max < Math.abs(pseudoDT[i])) max = pseudoDT[i];
        }
        if(max > 1) {
            for(int i = 0; i < VICTOR_COUNT; i++) {
                pseudoDT[i] /= max;
            }
        }
        for(int i = 0; i < VICTOR_COUNT; i++) {
            DT[i].set(pseudoDT[i]);
        }
    }   
}
